#include "base64.h"

static uint8_t base64_encode_table[] = {
    'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P',
    'Q','R','S','T','U','V','W','X','Y','Z','a','b','c','d','e','f',
    'g','h','i','j','k','l','m','n','o','p','q','r','s','t','u','v',
    'w','x','y','z','0','1','2','3','4','5','6','7','8','9','+','/',
};
static uint8_t base64_decode_table[] = {
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255, 62,255,255,255, 63,
	 52, 53, 54, 55, 56, 57, 58, 59, 60, 61,255,255,255,255,255,255,
	255,  0,  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14,
	 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25,255,255,255,255,255,
	255, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
	 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255
};

uint32_t base64_encode(uint8_t *psrc, uint32_t src_len, uint8_t *pdst, uint32_t dst_len) {
    register uint32_t i;
    register uint8_t m1, m0_2; //register
    if (src_len == 0) {
        return 0;
    }
    i = BASE64_ENCODE_LEN(src_len);
    if (i > dst_len) {
        return 0;
    }
    dst_len = i;
    for (i = 3; i <= src_len; i += 3) {
        m1 = psrc[i - 2]; //m1

        m0_2 = psrc[i - 3]; //m0
        *pdst = base64_encode_table[m0_2 >> 2]; pdst++;
        *pdst = base64_encode_table[((m0_2 & 0x03) << 4) | (m1 >> 4)]; pdst++;

        m0_2 = psrc[i - 1]; //m2
        *pdst = base64_encode_table[((m1 & 0x0F) << 2) | (m0_2 >> 6)]; pdst++;
        *pdst = base64_encode_table[m0_2 & 0x3F]; pdst++;
    }
    src_len = i - src_len;
    if (src_len == 1) {
        m0_2 = psrc[i - 3]; //m0
        m1 = psrc[i - 2];
        *pdst = base64_encode_table[m0_2 >> 2]; pdst++;
        *pdst = base64_encode_table[((m0_2 & 0x03) << 4) | (m1 >> 4)]; pdst++;
        *pdst = base64_encode_table[(m1 & 0x0F) << 2]; pdst++;
        *pdst = '=';
    } else if (src_len == 2) {
        m0_2 = psrc[i - 3]; //m0
        *pdst = base64_encode_table[m0_2 >> 2]; pdst++;
        *pdst = base64_encode_table[(m0_2 & 0x03) << 4]; pdst++;
        *pdst = '='; pdst++;
        *pdst = '=';
    }
    return dst_len;
}

uint32_t base64_decode(uint8_t *psrc, uint32_t src_len, uint8_t *pdst, uint32_t dst_len) {
    register uint32_t i;
    register uint8_t m0_2, m1_3; //register
    //check source length
    if ((src_len == 0) || (src_len & 0x03)) { //src_len % 4 != 0
        return 0;
    }
    i = BASE64_DECODE_LEN(src_len);
    if (i > dst_len) {
        return 0;
    }
    dst_len = i;

    //check and decode
    for (i = 0; i < src_len; i += 4) {
        m0_2 = base64_decode_table[*psrc]; //m0
        if (m0_2 >= 64) {
            return 0;
        }
        psrc++;

        m1_3 = base64_decode_table[*psrc]; //m1
        if (m1_3 >= 64) {
            return 0;
        }
        psrc++;

        *pdst = (m0_2 << 2) | (m1_3 >> 4); //{m0[5..0],m1[5..4]}
        pdst++;

        m0_2 = base64_decode_table[*psrc]; //m2
        if (m0_2 >= 64) {
            if (*psrc == '=') {
                dst_len--;
                m0_2 = 0;
            } else {
                return 0;
            }
        }
        psrc++;

        *pdst = ((m1_3 & 0x0F) << 4) | (m0_2 >> 2); //{m1[3..0],m2[5..2]}
        pdst++;

        m1_3 = base64_decode_table[*psrc]; //m3
        if (m1_3 >= 64) {
            if (*psrc == '=') {
                dst_len--;
                m1_3 = 0;
            } else {
                return 0;
            }
        }
        psrc++;

        *pdst = ((m0_2 & 0x03) << 6) | m1_3; //{m2[1..0],m3[5..0]}
        pdst++;
    }
    return dst_len;
}



